#! /usr/bin/env python
# -*- coding: utf-8 -*-

import time

import requests
from retry_decorator import retry


@retry(max_attempts=1, delay=1.0)
def set_action(action_name: str):
    url = "http://192.168.100.100:56322/rpc/aimdk.protocol.McActionService/SetAction"
    headers = {"Content-Type": "application/json"}
    payload = {
        "command": {"action": "McAction_USE_EXT_CMD", "ext_action": action_name},
    }
    rsp = requests.post(url, headers=headers, json=payload)
    rsp.raise_for_status()
    return rsp


@retry(max_attempts=1, delay=1.0)
def get_action():
    url = "http://192.168.100.100:56322/rpc/aimdk.protocol.McActionService/GetAction"
    headers = {"Content-Type": "application/json"}
    rsp = requests.post(url, headers=headers, json={})
    rsp.raise_for_status()
    return rsp.json()["info"]["current_action"]


def ensure_action(action_name: str, retries=5, retry_interval=1.0):
    try:
        for i in range(retries):
            if get_action() == action_name:
                return True
            set_action(action_name)
            time.sleep(0.1 if i == 0 else retry_interval)
    except Exception as e:
        print(f"Failed to set action: {e}")
        return False
    return False


def get_current_map_id():
    url = "http://192.168.100.110:50807/rpc/aimdk.protocol.MappingService/GetCurrentWorkingMap"
    headers = {"Content-Type": "application/json"}
    data = {"header": {}, "command": "MappingCommand_GET_CURRENT_WORKING_MAP"}
    rsp = requests.post(url, headers=headers, json=data)
    rsp.raise_for_status()
    return rsp.json()["data"]["map_id"]


def get_navi_points(map_id: int):
    url = "http://192.168.100.110:50807/rpc/aimdk.protocol.LocalizationService/GetTopoMsgs"
    headers = {"Content-Type": "application/json"}
    data = {"header": {}, "command": "TopoCommand_GET_TOPO_MSG", "map_id": map_id}
    rsp = requests.post(url, headers=headers, json=data)
    rsp.raise_for_status()
    return rsp.json()["data"]["points"]


@retry(max_attempts=3, delay=1.0)
def planning_navi_to_goal(map_id: int, target_id: int, task_id: int = 0):
    """Navigates to goal"""
    url = (
        "http://192.168.100.110:53176/rpc/aimdk.protocol.PncService/PlanningNaviToGoal"
    )
    headers = {"Content-Type": "application/json"}
    data = {
        "header": {
            "timestamp": {"seconds": 0, "nanos": 0, "ms_since_epoch": 0},
            "control_source": 0,
        },
        "task_id": task_id,
        "map_id": map_id,
        "target_id": target_id,
        "guide_line_id": 0,
        "ackerman_mode": False,
    }
    rsp = requests.post(url, headers=headers, json=data)
    rsp.raise_for_status()
    response_data = rsp.json()
    if response_data.get("state") != "CommonState_SUCCESS":
        raise Exception(
            f"Navigate task failed: {response_data.get('message', 'Unknown error')}")
    return response_data.get("task_id", task_id)


@retry(max_attempts=2, delay=0.5)
def get_pnc_task_state(task_id: int):
    """Gets PNC task state"""
    url = "http://192.168.100.110:53176/rpc/aimdk.protocol.PncService/ActionGetState"
    headers = {"Content-Type": "application/json"}
    data = {
        "header": {
            "timestamp": {"seconds": 0, "nanos": 0, "ms_since_epoch": 0},
            "control_source": 0,
        },
        "task_id": task_id,
    }
    rsp = requests.post(url, headers=headers, json=data)
    rsp.raise_for_status()
    return rsp.json()["state"]


@retry(max_attempts=2, delay=0.5)
def cancel_pnc_task(task_id: int):
    """Cancels PNC task"""
    url = "http://192.168.100.110:53176/rpc/aimdk.protocol.PncService/ActionCancel"
    headers = {"Content-Type": "application/json"}
    data = {
        "header": {
            "timestamp": {"seconds": 0, "nanos": 0, "ms_since_epoch": 0},
            "control_source": 0,
        },
        "task_id": task_id,
    }
    rsp = requests.post(url, headers=headers, json=data)
    rsp.raise_for_status()
    response_data = rsp.json()
    if response_data.get("state") != "CommonState_SUCCESS":
        raise Exception(
            f"Cancel task failed: {response_data.get('message', 'unknown error')}")
    return True


if __name__ == "__main__":
    # Gets current map ID
    current_map_id = get_current_map_id()
    print(f"Current map ID: {current_map_id}")

    # Gets current map navigation points
    navi_points = get_navi_points(current_map_id)
    print(f"Current map navi points: {navi_points}")

    # Chooses a navigation point
    for i, point in enumerate(navi_points):
        print(f"{i}: {point}")
    choice = input("Choose a navigation point: ")
    if choice.isdigit() and int(choice) >= 0 and int(choice) < len(navi_points):
        navi_point = navi_points[int(choice)]
        print(f"Choosed point: {navi_point}")
    else:
        print("Unvalid choice")
        exit(1)
    target_id = navi_point["point_id"]
    print(f"Choosed navigation target id: {target_id}")

    # Checks and switches to the required action state
    current_action = get_action()
    if current_action != "McAction_RL_LOCOMOTION_DEFAULT":
        print("Changing to McAction_RL_LOCOMOTION_DEFAULT...")
        exit(1)
    if not ensure_action("McAction_RL_LOCOMOTION_DEFAULT"):
        print("Changing to McAction_RL_LOCOMOTION_DEFAULT failed")
        exit(1)

    # Executes navigation task
    try:
        task_id = planning_navi_to_goal(int(current_map_id), int(target_id))
        print(f"Navigate task sent, task id: {task_id}")
    except Exception as e:
        print(f"Send navigate task failed: {e}")
        exit(1)

    # Waits for navigation task to complete
    try:
        while True:
            pnc_state = get_pnc_task_state(task_id)
            print(f"Current pnc state: {pnc_state}")

            if pnc_state == "PncServiceState_SUCCESS":
                print("Navigate task completed")
                break
            elif pnc_state == "PncServiceState_FAILED":
                print("Navigate task failed")
                break
            elif pnc_state == "PncServiceState_RUNNING":
                print("Navigating...")
            elif pnc_state == "PncServiceState_PAUSED":
                print("Navigation paused")
            elif pnc_state == "PncServiceState_IDLE":
                print("PNC idle")
                break

            time.sleep(1)
    except KeyboardInterrupt:
        print("\nReceived keyboard interrupt, canceling task...")
        try:
            cancel_pnc_task(task_id)
            print("Navigate task canceled")
        except Exception as e:
            print(f"Navigate task canceled failed: {e}")
    except Exception as e:
        print(f"Task failed: {e}")
